
//dit is de motorfile
//comm wat doet de functie,welke variabelen verwacht,wat die de functie, wat geeft het terug, namen en comm in het engels staat in de h file
//private variabelen eindigen met een _
//1 singletong klasse maken "serieel" 1 klasse motor waarvan er twee objecten worden gemaakt, één voor elk wiel
//bij USB aansluiten van het Wim bordje zit op /dev/ttyUSB0
#include "motor.h"



Motor::Motor()
{
    logger = new Log("Motor");
    port_ = new Serial(DEVICE, BAUDRATE);
    //initialize the serial port
}

Motor::~Motor()
{
    logger->stuur("destructor called");
    port_->closePort();
    delete port_;
    delete logger;
}

void Motor::setSpeed(bool direction, int speed)
{
    logger->stuur("setting the speed");
    //sets the speed of BOTH motors in the same direction (forward/reverse)
    //example
    if(direction==true){
    sendCommand(FORWARD, speed, speed);
    }
    else{
    sendCommand(REVERSE, speed, speed);
    }
}

void Motor::drive_individual(bool direction, int speed1, int speed2)
{
    logger->stuur("setting the speed of individual motors");
    //sets the speed of individual motors in the same direction (forward/reverse)
    //example
    if(direction==true){
    sendCommand(FORWARD_IND, speed1, speed2);
    }
    else{
    sendCommand(REVERSE_IND, speed1, speed2);
    }
}


void Motor::rotate(bool clockwise, int speed)
{
    logger->stuur("rotating");
    //let the robot turn clockwise or counterclockwise
    if(clockwise==true){
    sendCommand(TURN_CW, speed, speed);
    }
    else{
    sendCommand(TURN_CCW, speed, speed);
    }

}

void Motor::drive_left(bool direction, int speed)
{
    logger->stuur("driving left wheel");
    //let the robot turn clockwise or counterclockwise
    if(direction==true){
    sendCommand(M1_FORWARD, speed, speed);
    }
    else{
    sendCommand(M1_REVERSE, speed, speed);
    }

}



void Motor::drive_right(bool direction, int speed)
{
    logger->stuur("driving right wheel");
    //let the robot turn clockwise or counterclockwise
    if(direction==false){
    sendCommand(M2_FORWARD, speed, speed);
    }
    else{
    sendCommand(M2_REVERSE, speed, speed);
    }

}

void Motor::brake_left(bool direction, int speed)
{
    logger->stuur("driving left wheel");
    //let the robot turn clockwise or counterclockwise
    if(direction==true){
    sendCommand(M1_BRAKE_LOW_FWD, speed, speed);
    }
    else{
    sendCommand(M1_BRAKE_LOW_RVS, speed, speed);
    }

}


void Motor::brake_right(bool direction, int speed)
{
    logger->stuur("stopping right wheel");
    //let the robot turn clockwise or counterclockwise
    if(direction==false){
    sendCommand(M2_BRAKE_LOW_FWD, speed, speed);
    }
    else{
    sendCommand(M2_BRAKE_LOW_RVS, speed, speed);
    }

}



void Motor::stop()
{
    logger->stuur("stopping");
    //Let all motors stop
}

bool Motor::sendCommand(unsigned char code, float data1, float data2)
{
    logger->stuur("stuur commando");
    int result;
    unsigned char xd2[3];

    xd2[0] = code;
    xd2[1] = data1;
    xd2[2] = data2;

    result = port_->initPort();

    if(result > 0)
    {
        port_->sendData(xd2,3);
        sleep(1);   //Moet dit hier nog staan?
        port_->closePort();
        return true;
    }
    else
    {
        return false;
    }
}
